#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <wiringPi.h>
#include <wiringSerial.h>
#include <softPwm.h>

// 定义继电器引脚 (使用wiringPi编号)
#define RELAY1_PIN 0  // 对应物理引脚11
#define RELAY2_PIN 1  // 对应物理引脚12
#define RELAY3_PIN 2  // 对应物理引脚13

#define LED1 7        // 对应物理引脚7
#define LED2 0        // 对应物理引脚11

// 定义舵机PWM引脚和参数
#define SERVO1_PIN 1  // 对应物理引脚12 (必须是硬件PWM引脚)
#define SERVO2_PIN 2  // 对应物理引脚13 (必须是硬件PWM引脚)

// 舵机PWM参数 (50Hz频率，周期20ms)
#define PWM_FREQ 50
#define PWM_RANGE 2000 // 20ms = 2000us

// 舵机角度映射 (角度到PWM值的映射)
const int initialPWM = 250;  // 对应180度 (180度 = 2.5ms = 250us)
const int targetPWM = 125;   // 对应80度 (80度 = 1.25ms = 125us)

// 全局变量
int serialHandle;            // 串口句柄
unsigned long delayStartTime = 0;  // 记录延迟开始的时间
bool delayInProgress = false;      // 延迟是否在进行中
const unsigned long delayDuration = 8230;  // 延迟时间(毫秒)

// 函数声明
void setup();
void loop();
void handleYellowRiceDetection();
unsigned long millis();

int main(void) {
    // 初始化wiringPi库
    if (wiringPiSetup() == -1) {
        fprintf(stderr, "无法初始化wiringPi库\n");
        return 1;
    }
    
    // 初始化设置
    setup();
    
    // 主循环
    while (1) {
        loop();
        usleep(10000);  // 10ms延迟，减少CPU使用率
    }
    
    return 0;
}

void setup() {
    // 初始化串口通信 (与OpenMV通信)
    if ((serialHandle = serialOpen("/dev/ttySAC1", 115200)) < 0) {
        fprintf(stderr, "无法打开串口\n");
        exit(1);
    }
    
    // 设置LED引脚为输出模式
    pinMode(LED1, OUTPUT);
    pinMode(LED2, OUTPUT);
    
    // 设置继电器引脚为输出模式
    pinMode(RELAY1_PIN, OUTPUT);
    pinMode(RELAY2_PIN, OUTPUT);
    pinMode(RELAY3_PIN, OUTPUT);
    
    // 初始状态：继电器1和2打开，继电器3关闭
    digitalWrite(RELAY1_PIN, LOW);
    digitalWrite(RELAY2_PIN, LOW);
    digitalWrite(RELAY3_PIN, HIGH);
    
    // 初始LED状态
    digitalWrite(LED1, HIGH);
    digitalWrite(LED2, HIGH);
    
    // 初始化舵机控制 (使用硬件PWM)
    pwmSetMode(PWM_MODE_MS);      // 设置为标记-空间模式
    pwmSetClock(384);              // 基础时钟分频 (19.2MHz / 384 = 50kHz)
    pwmSetRange(PWM_RANGE);       // 设置PWM范围
    
    // 设置舵机初始位置
    pwmWrite(SERVO1_PIN, initialPWM);
    pwmWrite(SERVO2_PIN, initialPWM);
    
    // 等待系统稳定
    delay(1000);
}

void loop() {
    // 检查串口是否有数据
    if (serialDataAvail(serialHandle) > 0) {
        // 读取接收到的数据
        int receivedData = serialGetchar(serialHandle);
        
        // 如果接收到数字49（ASCII码对应字符'1'）且当前没有延迟在进行
        if (receivedData == 49 && !delayInProgress) {
            // 开始延迟计时
            delayStartTime = millis();
            delayInProgress = true;
            
            // 发送确认信息回OpenMV
            // serialPuts(serialHandle, "Delay started, relay will activate in 8.23 seconds\n");
        }
    }
    
    // 检查延迟是否完成
    if (delayInProgress && (millis() - delayStartTime >= delayDuration)) {
        // 打开继电器
        handleYellowRiceDetection();
        delayInProgress = false;
        
        // 发送继电器已激活的信息
        serialPuts(serialHandle, "Relay activated after 8.23 seconds\n");
    }
    
    // 保持LED始终亮着
    digitalWrite(LED1, HIGH);
    digitalWrite(LED2, HIGH);
}

void handleYellowRiceDetection() {
    // 1. 继电器1和2停止（假设低电平为停止）
    digitalWrite(RELAY1_PIN, HIGH);
    digitalWrite(RELAY2_PIN, HIGH);
    
    // 2. 舵机旋转到目标角度
    pwmWrite(SERVO1_PIN, targetPWM);
    pwmWrite(SERVO2_PIN, targetPWM);
    delay(1000); // 等待舵机到位
    
    // 3. 继电器3运行3秒
    digitalWrite(RELAY3_PIN, LOW);
    delay(3000);
    digitalWrite(RELAY3_PIN, HIGH);
    
    // 4. 舵机恢复初始状态
    pwmWrite(SERVO1_PIN, initialPWM);
    pwmWrite(SERVO2_PIN, initialPWM);
    delay(1000); // 等待舵机到位
    
    // 5. 继电器1和2重新打开
    digitalWrite(RELAY1_PIN, LOW);
    digitalWrite(RELAY2_PIN, LOW);
}

// 模拟Arduino的millis()函数
unsigned long millis() {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (unsigned long)(tv.tv_sec * 1000 + tv.tv_usec / 1000);
}
